#include <ros/init.h>
#include <ros/package.h>

#include <ocs2_ddp/GaussNewtonDDP_MPC.h>
#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
#include "mpc_quadrotor_mujoco/QuadrotorInterface.h"

int main(int argc, char** argv) {
    const std::string robotName = "quadrotor";

    std::vector<std::string> programArgs{};
    ::ros::removeROSArgs(argc, argv, programArgs);

    if (programArgs.size() <= 1) {
        throw std::runtime_error("No task file specified. Aborting.");
    }
    std::string taskFileFolderName(programArgs[1]);

    ros::init(argc, argv, robotName + "_mpc");
    ros::NodeHandle nodeHandle;

    const std::string taskFile = ros::package::getPath("mpc_quadrotor") + "/config/" + taskFileFolderName + "/task.info";
    const std::string libFolder = ros::package::getPath("mpc_quadrotor") + "/auto_generated";
    ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder);

    auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, quadrotorInterface.getReferenceManagerPtr());
    rosReferenceManagerPtr->subscribe(nodeHandle);

    ocs2::GaussNewtonDDP_MPC mpc(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), quadrotorInterface.getRollout(),
                                quadrotorInterface.getOptimalControlProblem(), quadrotorInterface.getInitializer());
    mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);

    ocs2::MPC_ROS_Interface mpcNode(mpc, robotName);
    mpcNode.launchNodes(nodeHandle);

    return 0;
}
